
#ifndef ISPACETREE_H
#define ISPACETREE_H

#include <BValue>
#include <BSpace>
#include <BMatrix>

namespace BWE
{
	struct SpaceNode
	{
		inline SpaceNode(SpaceNode* parent = 0)
		{
			this->parent = parent;
		}
		BSpace		space;
		SpaceNode*	parent;
		SpaceNode*	prev = 0;
		SpaceNode*	next = 0;
		BValue		value;
	};
	struct SpacePair
	{
		const SpaceNode* nodeA;
		const SpaceNode* nodeB;
		SpacePair()
		{
			nodeA = 0;
			nodeB = 0;
		}
		SpacePair(const SpaceNode* na, const SpaceNode* nb)
		{
			nodeA = na;
			nodeB = nb;
		}
	};

	typedef BArray<SpaceNode*>	SpaceNodeArray;
	typedef BArray<SpacePair>	SpacePairArray;

	class DbvntNode
	{
	public:
		DbvntNode(const SpaceNode* node) : space(node->space)
		{
			angle = 0;
			value = node->value;
			prev = 0;
			next = 0;
		}
		~DbvntNode()
		{
			if (prev)
				delete prev;
			if (next)
				delete next;
		}

		inline bool isLeaf() const
		{
			return next == 0;
		}
		inline bool isInternal() const
		{
			return next != 0;
		}

		BSpace	space;
		BVector	normal;
		BReal		angle;
		BValue		value;
		DbvntNode* prev;
		DbvntNode* next;
	};
	struct DbvntPair
	{
		const DbvntNode* a;
		const DbvntNode* b;
		DbvntPair()
		{
			a = 0;
			b = 0;
		}
		DbvntPair(const DbvntNode* na, const DbvntNode* nb)
		{
			a = na;
			b = nb;
		}
	};

	typedef BArray<DbvntNode*>	DbvntNodeArray;
	typedef BArray<DbvntPair>	DbvntPairArray;

	class ISpaceTree
	{
	public:
		inline ISpaceTree()
		{
			_root = 0;
		}
		inline~ISpaceTree()
		{
			clear();
		}

		inline bool empty() const
		{
			return _root == 0;
		}
		inline void clear()
		{
			if (_root)
			{
				recurseDeleteNode(_root);
				_root = 0;
			}
		}
		inline void reset(SpaceNode* root)
		{
			clear();
			_root = root;
		}

		inline SpaceNode* root()
		{
			return _root;
		}
		inline const SpaceNode* root() const
		{
			return _root;
		}

		inline SpaceNode* insert(const BSpace& space, const BValue& value)
		{
			SpaceNode* leaf = createNode(0, space, value);
			insertNode(_root, leaf);
			return (leaf);
		}
		inline SpaceNode* remove(SpaceNode* leaf)
		{
			if (leaf == _root)
			{
				_root = 0;
				return 0;
			}
			SpaceNode* parent = leaf->parent;
			SpaceNode* node = parent->parent;
			SpaceNode* sibling = 0;
			if (leaf->parent->prev != leaf)
				sibling = parent->prev;
			else
				sibling = parent->next;
			if (node)
			{
				if (parent->parent->prev == parent)
					node->prev = sibling;
				else
					node->next = sibling;
				sibling->parent = node;
				delete parent;
				while (node)
				{
					const BSpace space = node->space;
					node->space = node->prev->space | node->next->space;
					if (space == node->space)
						return node;
					node = node->parent;
				}
				return _root;
			}
			_root = sibling;
			sibling->parent = 0;
			delete parent;
			return _root;
		}

		inline bool update(SpaceNode* leaf, const BSpace& space, const BVector& delta, BReal margin = 0)
		{
			if (leaf->space.contain(space))
				return false;
			SpaceNode* root = this->remove(leaf);
			BSpace aabb = space;
			aabb.min() -= margin;
			aabb.max() += margin;
			if (delta.x() > 0)
				aabb.max().x() += delta.x();
			else
				aabb.min().x() += delta.x();
			if (delta.y() > 0)
				aabb.max().y() += delta.y();
			else
				aabb.min().y() += delta.y();
			if (delta.z() > 0)
				aabb.max().z() += delta.z();
			else
				aabb.min().z() += delta.z();
			leaf->space = aabb;
			this->insertNode(root, leaf);
			return true;
		}
		inline bool update(SpaceNode* leaf, const BSpace& space, BReal margin = 0)
		{
			if (leaf->space.contain(space))
				return false;
			SpaceNode* root = this->remove(leaf);
			BSpace aabb = space;
			aabb.min() -= margin;
			aabb.max() += margin;
			leaf->space = aabb;
			this->insertNode(root, leaf);
			return true;
		}

		inline bool collide(SpaceNode* node0, SpaceNode* node1, SpacePairArray& result)
		{
			result.reset();
			if (node0 && node1)
			{
				int depth = 1;
				int treshold = 128 - 4;
				BArray<SpacePair> stkStack;
				stkStack.resize(128);
				stkStack[0] = SpacePair(node0, node1);
				do
				{
					SpacePair pair = stkStack[--depth];
					if (depth > treshold)
					{
						stkStack.resize(stkStack.size() * 2);
						treshold = stkStack.size() - 4;
					}
					if (pair.nodeA == pair.nodeB)
					{
						if (pair.nodeA->next)
						{
							stkStack[depth++] = SpacePair(pair.nodeA->prev, pair.nodeA->prev);
							stkStack[depth++] = SpacePair(pair.nodeA->next, pair.nodeA->next);
							stkStack[depth++] = SpacePair(pair.nodeA->prev, pair.nodeA->next);
						}
					}
					else if (pair.nodeA->space.intersect(pair.nodeB->space))
					{
						if (pair.nodeA->next)
						{
							if (pair.nodeB->next)
							{
								stkStack[depth++] = SpacePair(pair.nodeA->prev, pair.nodeB->prev);
								stkStack[depth++] = SpacePair(pair.nodeA->next, pair.nodeB->prev);
								stkStack[depth++] = SpacePair(pair.nodeA->prev, pair.nodeB->next);
								stkStack[depth++] = SpacePair(pair.nodeA->next, pair.nodeB->next);
							}
							else
							{
								stkStack[depth++] = SpacePair(pair.nodeA->prev, pair.nodeB);
								stkStack[depth++] = SpacePair(pair.nodeA->next, pair.nodeB);
							}
						}
						else
						{
							if (pair.nodeB->next)
							{
								stkStack[depth++] = SpacePair(pair.nodeA, pair.nodeB->prev);
								stkStack[depth++] = SpacePair(pair.nodeA, pair.nodeB->next);
							}
							else
							{
								result.append(pair);
							}
						}
					}
				} while (depth);
			}
			return result.size();
		}
		inline bool collide(SpaceNode* node, const BSpace& space, SpaceNodeArray& result) const
		{
			result.reset();
			if (node)
			{
				SpaceNodeArray stack;
				stack.reserve(64);
				stack.append(node);
				do
				{
					SpaceNode* node = stack[stack.size() - 1];
					stack.pop();
					if (node->space.intersect(space))
					{
						if (node->next)
						{
							stack.append(node->prev);
							stack.append(node->next);
						}
						else
						{
							result.append(node);
						}
					}
				} while (stack.size());
			}
			return result.size();
		}

		inline void selfCollide(const DbvntNode* root, DbvntPairArray& result)
		{
			if (root)
			{
				int depth = 1;
				int treshold = 128 - 4;
				DbvntPairArray stkStack;
				stkStack.resize(128);
				stkStack[0] = DbvntPair(root, root);
				do
				{
					DbvntPair pair = stkStack[--depth];
					if (depth > treshold)
					{
						stkStack.resize(stkStack.size() * 2);
						treshold = stkStack.size() - 4;
					}
					if (pair.a == pair.b)
					{
						if (pair.a->isInternal() && pair.a->angle > PI)
						{
							stkStack[depth++] = DbvntPair(pair.a->prev, pair.a->prev);
							stkStack[depth++] = DbvntPair(pair.a->next, pair.a->next);
							stkStack[depth++] = DbvntPair(pair.a->prev, pair.a->next);
						}
					}
					else if (pair.a->space.intersect(pair.b->space))
					{
						if (pair.a->isInternal())
						{
							if (pair.b->isInternal())
							{
								stkStack[depth++] = DbvntPair(pair.a->prev, pair.b->prev);
								stkStack[depth++] = DbvntPair(pair.a->next, pair.b->prev);
								stkStack[depth++] = DbvntPair(pair.a->prev, pair.b->next);
								stkStack[depth++] = DbvntPair(pair.a->next, pair.b->next);
							}
							else
							{
								stkStack[depth++] = DbvntPair(pair.a->prev, pair.b);
								stkStack[depth++] = DbvntPair(pair.a->next, pair.b);
							}
						}
						else
						{
							if (pair.b->isInternal())
							{
								stkStack[depth++] = DbvntPair(pair.a, pair.b->prev);
								stkStack[depth++] = DbvntPair(pair.a, pair.b->next);
							}
							else
							{
								result.append(pair);
							}
						}
					}
				} while (depth);
			}
		}

		void rayTest(const BVector& rayFrom, const BVector& rayTo, const BVector& rayDirectionInverse, BUInt signs[3], BReal lambda_max, const BSpace& space, SpaceNodeArray& result)
		{
			if (_root)
			{
				BVector resultNormal;

				int depth = 1;
				int treshold = 128 - 2;
				SpaceNodeArray stack;
				stack.reserve(64);
				stack.append(_root);
				BVector bounds[2];
				do
				{
					SpaceNode* node = stack[--depth];
					bounds[0] = node->space.min() - space.max();
					bounds[1] = node->space.max() - space.min();
					BReal tmin = 1.f;
					BReal lambda_min = 0.f;
					unsigned int result1 = false;
					result1 = rayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
					if (result1)
					{
						if (node->next)
						{
							if (depth > treshold)
							{
								stack.resize(stack.size() * 2);
								treshold = stack.size() - 2;
							}
							stack[depth++] = node->prev;
							stack[depth++] = node->next;
						}
						else
						{
							result.append(node);
						}
					}
				} while (depth);
			}
		}

	private:
		inline void insertNode(SpaceNode* root, SpaceNode* leaf)
		{
			if (!_root)
			{
				_root = leaf;
				leaf->parent = 0;
				return;
			}
			while (root->next)
			{
				const BSpace& prevSpace = root->prev->space;
				BVector da = leaf->space.center() - prevSpace.center();
				BReal va = fabs(da.x()) + fabs(da.y()) + fabs(da.z());

				const BSpace& nextSpace = root->next->space;
				BVector db = leaf->space.center() - nextSpace.center();
				BReal vb = fabs(db.x()) + fabs(db.y()) + fabs(db.z());

				if (va < vb)
					root = root->prev;
				else
					root = root->next;
			}
			SpaceNode* prev = root->parent;
			SpaceNode* node = createNode(prev, leaf->space, root->space, 0);
			if (prev)
			{
				if (root->parent->prev == root)
					prev->prev = node;
				else
					prev->next = node;
				node->prev = root;
				root->parent = node;
				node->next = leaf;
				leaf->parent = node;
				do
				{
					if (prev->space.contain(node->space))
						break;
					prev->space = prev->prev->space | prev->next->space;
					node = prev;
				} while (0 != (prev = node->parent));
			}
			else
			{
				node->prev = root;
				root->parent = node;
				node->next = leaf;
				leaf->parent = node;
				_root = node;
			}
		}

		inline SpaceNode* createNode(SpaceNode* parent, const BSpace& space, const BValue& value)
		{
			SpaceNode* node = new SpaceNode(parent);
			node->value = value;
			node->space = space;
			return node;
		}
		inline SpaceNode* createNode(SpaceNode* parent, const BSpace& space0, const BSpace& space1, const BValue& value)
		{
			SpaceNode* node = new SpaceNode(parent);
			node->value = value;
			node->space = space0 | space1;
			return node;
		}

		inline void recurseDeleteNode(SpaceNode* node)
		{
			if (node == 0)
				return;
			if (node->next)
			{
				recurseDeleteNode(node->prev);
				recurseDeleteNode(node->next);
			}
			if (node == _root)
				_root = 0;
			delete node;
		}

		inline bool rayAabb2(const BVector& rayFrom,
			const BVector& rayInvDirection,
			const BUInt raySign[3],
			const BVector bounds[2],
			BReal& tmin,
			BReal lambda_min,
			BReal lambda_max)
		{
			BReal tmax, tymin, tymax, tzmin, tzmax;
			tmin = (bounds[raySign[0]].x() - rayFrom.x()) * rayInvDirection.x();
			tmax = (bounds[1 - raySign[0]].x() - rayFrom.x()) * rayInvDirection.x();
			tymin = (bounds[raySign[1]].y() - rayFrom.y()) * rayInvDirection.y();
			tymax = (bounds[1 - raySign[1]].y() - rayFrom.y()) * rayInvDirection.y();

			if ((tmin > tymax) || (tymin > tmax))
				return false;

			if (tymin > tmin)
				tmin = tymin;

			if (tymax < tmax)
				tmax = tymax;

			tzmin = (bounds[raySign[2]].z() - rayFrom.z()) * rayInvDirection.z();
			tzmax = (bounds[1 - raySign[2]].z() - rayFrom.z()) * rayInvDirection.z();

			if ((tmin > tzmax) || (tzmin > tmax))
				return false;
			if (tzmin > tmin)
				tmin = tzmin;
			if (tzmax < tmax)
				tmax = tzmax;
			return ((tmin < lambda_max) && (tmax > lambda_min));
		}

	private:
		SpaceNode*	_root;
	};
}

#endif
